/***************************** Include Files ********************************/

#include "xsysmon.h"
#include "xparameters.h"
#include "xstatus.h"
#include "stdio.h"
#include "../inc/sysctl_config.h"

static XSysMon SysMonInst;      /* System Monitor driver instance */


/****************************************************************************/
/*
*
* This function converts the fraction part of the given floating point number
* (after the decimal point)to an integer.
*
* @param	FloatNum is the floating point number.
*
* @return	Integer number to a precision of 3 digits.
*
* @note
* This function is used in the printing of floating point data to a STDIO device
* using the xil_printf function. The xil_printf is a very small foot-print
* printf function and does not support the printing of floating point numbers.
*
*****************************************************************************/
int SysMonFractionToInt(float FloatNum)
{
	float Temp;

	Temp = FloatNum;
	if (FloatNum < 0) {
		Temp = -(FloatNum);
	}

	return( ((int)((Temp -(float)((int)Temp)) * (1000.0f))));
}




int init_sysmon(u16 SysMonDeviceId)
{
	int Status;
	XSysMon_Config *ConfigPtr;

	XSysMon *SysMonInstPtr = &SysMonInst;

//	printf("\r\n*****Entering the SysMon Polled Example. \r\n");

	/*
	 * Initialize the SysMon driver.
	 */
	ConfigPtr = XSysMon_LookupConfig(SysMonDeviceId);
	if (ConfigPtr == NULL) {
		return XST_FAILURE;
	}
	XSysMon_CfgInitialize(SysMonInstPtr, ConfigPtr,
				ConfigPtr->BaseAddress);

	/*
	 * Disable the Channel Sequencer before configuring the Sequence
	 * registers.
	 */
	XSysMon_SetSequencerMode(SysMonInstPtr, XSM_SEQ_MODE_SINGCHAN);


	/*
	 * Disable all the alarms in the Configuration Register 1.
	 */
	XSysMon_SetAlarmEnables(SysMonInstPtr, 0x0);


	/*
	 * Setup the Averaging to be done for the channels in the
	 * Configuration 0 register as 16 samples:
	 */
	XSysMon_SetAvg(SysMonInstPtr, XSM_AVG_16_SAMPLES);


	/*
	 * Enable the averaging on the following channels in the Sequencer
	 * registers:
	 * 	- On-chip Temperature, VCCINT/VCCAUX  supply sensors
	 * 	- 1st/16th Auxiliary Channels
	  *	- Calibration Channel
	 */
	Status =  XSysMon_SetSeqAvgEnables(SysMonInstPtr, XSM_SEQ_CH_TEMP |
						XSM_SEQ_CH_VCCINT |
						XSM_SEQ_CH_VCCAUX |
						XSM_SEQ_CH_CALIB);
	if (Status != XST_SUCCESS) {
		return XST_FAILURE;
	}

	/*
	 * Enable the following channels in the Sequencer registers:
	 * 	- On-chip Temperature, VCCINT/VCCAUX supply sensors
	 * 	- 1st/16th Auxiliary Channel
	 *	- Calibration Channel
	 */
	Status =  XSysMon_SetSeqChEnables(SysMonInstPtr, XSM_SEQ_CH_TEMP |
						XSM_SEQ_CH_VCCINT |
						XSM_SEQ_CH_VCCAUX |
						XSM_SEQ_CH_CALIB);
	if (Status != XST_SUCCESS) {
		return XST_FAILURE;
	}


	/*
	 * Set the ADCCLK frequency equal to 1/32 of System clock for the System
	 * Monitor/ADC in the Configuration Register 2.
	 */
	XSysMon_SetAdcClkDivisor(SysMonInstPtr, 32);


	/*
	 * Set the Calibration enables.
	 */
	XSysMon_SetCalibEnables(SysMonInstPtr,
				XSM_CFR1_CAL_PS_GAIN_OFFSET_MASK |
				XSM_CFR1_CAL_ADC_GAIN_OFFSET_MASK);

	/*
	 * Enable the Channel Sequencer in continuous sequencer cycling mode.
	 */
	XSysMon_SetSequencerMode(SysMonInstPtr, XSM_SEQ_MODE_CONTINPASS);

	/*
	 * Wait till the End of Sequence occurs
	 */
	XSysMon_GetStatus(SysMonInstPtr); /* Clear the old status */
	while ((XSysMon_GetStatus(SysMonInstPtr) & XSM_SR_EOS_MASK) !=
			XSM_SR_EOS_MASK);
			
	return XST_SUCCESS;
}

void read_sysmon(int *TempData, int *VccAuxData, int *VccIntData)
{
	int Status;
	XSysMon_Config *ConfigPtr;
	u32 TempRawData;
	u32 VccAuxRawData;
	u32 VccIntRawData;
	
	float TempData_f;
	float VccAuxData_f;
	float VccIntData_f;
	
	float MaxData;
	float MinData;
	XSysMon *SysMonInstPtr = &SysMonInst;
	
	/*
	 * Read the on-chip Temperature Data (Current/Maximum/Minimum)
	 * from the ADC data registers.
	 */
	TempRawData = XSysMon_GetAdcData(SysMonInstPtr, XSM_CH_TEMP);
	TempData_f = XSysMon_mRawToTemperature(TempRawData);

#ifdef DEBUG
	printf("\r\nThe Current Temperature is %0d.%03d Centigrades.\r\n",
				(int)(TempData_f), SysMonFractionToInt(TempData_f));

	xil_printf("TempData_f: %d\n\r", (int)TempData_f);
#endif


//	TempRawData = XSysMon_GetMinMaxMeasurement(SysMonInstPtr, XSM_MAX_TEMP);
//	MaxData = XSysMon_mRawToTemperature(TempRawData);
//	printf("The Maximum Temperature is %0d.%03d Centigrades. \r\n",
//				(int)(MaxData), SysMonFractionToInt(MaxData));
//
//	TempRawData = XSysMon_GetMinMaxMeasurement(SysMonInstPtr, XSM_MIN_TEMP);
//	MinData = XSysMon_mRawToTemperature(TempRawData);
//	printf("The Minimum Temperature is %0d.%03d Centigrades. \r\n",
//				(int)(MinData), SysMonFractionToInt(MinData));

	/*
	 * Read the VccInt Votage Data (Current/Maximum/Minimum) from the
	 * ADC data registers.
	 */
	VccIntRawData = XSysMon_GetAdcData(SysMonInstPtr, XSM_CH_VCCINT);
	VccIntData_f = XSysMon_mRawToVoltage(VccIntRawData);

#ifdef DEBUG
	printf("\r\nThe Current VCCINT is %0d.%03d Volts. \r\n",
			(int)(VccIntData_f), SysMonFractionToInt(VccIntData_f));
#endif

//	VccIntRawData = XSysMon_GetMinMaxMeasurement(SysMonInstPtr,
//							XSM_MAX_VCCINT);
//	MaxData = XSysMon_mRawToVoltage(VccIntRawData);
//	printf("The Maximum VCCINT is %0d.%03d Volts. \r\n",
//			(int)(MaxData), SysMonFractionToInt(MaxData));
//
//	VccIntRawData = XSysMon_GetMinMaxMeasurement(SysMonInstPtr,
//							XSM_MIN_VCCINT);
//	MinData = XSysMon_mRawToVoltage(VccIntRawData);
//	printf("The Minimum VCCINT is %0d.%03d Volts. \r\n",
//			(int)(MinData), SysMonFractionToInt(MinData));

	/*
	 * Read the VccAux Votage Data (Current/Maximum/Minimum) from the
	 * ADC data registers.
	 */
	VccAuxRawData = XSysMon_GetAdcData(SysMonInstPtr, XSM_CH_VCCAUX);
	VccAuxData_f = XSysMon_mRawToVoltage(VccAuxRawData);
#ifdef DEBUG
	printf("\r\nThe Current VCCAUX is %0d.%03d Volts. \r\n",
			(int)(VccAuxData_f), SysMonFractionToInt(VccAuxData_f));
#endif

//	VccAuxRawData = XSysMon_GetMinMaxMeasurement(SysMonInstPtr,
//							XSM_MAX_VCCAUX);
//	MaxData = XSysMon_mRawToVoltage(VccAuxRawData);
//	printf("The Maximum VCCAUX is %0d.%03d Volts. \r\n",
//				(int)(MaxData), SysMonFractionToInt(MaxData));
//
//
//	VccAuxRawData = XSysMon_GetMinMaxMeasurement(SysMonInstPtr,
//							XSM_MIN_VCCAUX);
//	MinData = XSysMon_mRawToVoltage(VccAuxRawData);
//	printf("The Minimum VCCAUX is %0d.%03d Volts. \r\n\r\n",
//				(int)(MinData), SysMonFractionToInt(MinData));

	*TempData = (TempData_f < 0) ? 0xFF : (int)TempData_f;
	*VccAuxData = (int)(VccAuxData_f*50+0.5);
	*VccIntData = (int)(VccIntData_f*50+0.5);
}

